#include<string.h>
#include<stdint.h>
#include<stdlib.h>
#include <icommon.h>
#include <stdio.h>
#include <sys/time.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
static void c_resize_c_nv12_up_h_720_960p_onlynear(uint8_t* src, uint8_t* dst, int src_w, int src_h, int dst_w, int dst_h, int src_step, int src_height, uint16_t *resize_buf)
{
	int i;

	float scaley = 1.0 * src_h/dst_h;
	int uv_height = dst_h >> 1;
	int width  = dst_w;
	int fy = 0;

	unsigned char *src_data = NULL;
	unsigned char *vrd_data = NULL;
	for(i = 0 ; i < dst_h; i++) {
		fy = i * scaley;
		vrd_data = dst + i*dst_w;
		src_data  = src + fy*src_w;
		memcpy(vrd_data,src_data,width);
	}
	dst += dst_w * dst_h;
	src += src_w * src_h;
	for(i = 0 ; i < uv_height; i++) {
		fy = i * scaley;
		vrd_data = dst + i*dst_w;
		src_data  = src + fy*src_w;
		memcpy(vrd_data,src_data,width);
	}
}

//buf = (unsigned short*)malloc(sizeof(unsigned short) * (dst_h * 6 + dst_w * 4 + C_MAX_WH)/*C_MAX_WH * 9*/);
static void c_resize_c_nv12(uint8_t* src, uint8_t* dst, int src_w, int src_h, int dst_w, int dst_h, int src_step, int src_height, uint16_t *resize_buf)
{
	int i,j,l,l1;
	float scalex = 1.0 * src_w / dst_w;
	float scaley = 1.0 * src_h / dst_h;

	int height = dst_h + (dst_h >> 1);
	int width  = dst_w;
	unsigned short *x_weightl, *x_weightr, *y_weightl, *y_weightr, *coordx, *coordx1, *coordy, *coordy1;

	x_weightl = resize_buf;
	x_weightr = x_weightl + dst_w;
	y_weightl = x_weightr + dst_w;
	y_weightr = y_weightl + dst_h;
	coordx 	  = y_weightr + dst_h;
	coordx1   = coordx    +  dst_w;
	coordy    = coordx1   +  dst_w;
	coordy1   = coordy    +  dst_h;


	float fx, fy;
	int ix, iy;


	for(i = 0; i < width; i++)
	{
		fx = i * scalex;
		ix = (int)fx;
		x_weightr[i] = (unsigned short)((fx - ix)*16 + 0.5);
		x_weightl[i] = 16 - x_weightr[i];
		coordx[i] = ix;
		if(ix < src_w - 1)
			coordx1[i] = ix + 1;
		else coordx1[i] = ix;
	}

	for(j = 0 ; j < dst_h; j++)
	{
		fy = j * scaley;
		iy = (int)fy;
		y_weightr[j] = (unsigned short)((fy - iy)*16 + 0.5);
		y_weightl[j] = 16 - y_weightr[j];
		coordy[j] = iy;
		if(iy < src_h - 1) coordy1[j] = iy + 1;
		else coordy1[j] = iy;
	}

	const unsigned char* data;
	const unsigned char* data1;
	unsigned char* vrd_data;

	//------------------------Y--------------------------------------------------//
	for(i = 0; i < dst_h; i++)
	  {//for1

	    vrd_data = dst + dst_w * i;

	    if(y_weightr[i])
	      {//if
		l = 0;
		data = src + src_step* coordy[i];
		data1 = src + src_step * coordy1[i];
		for(l1 = 0 ; l1 < width; l1++,l++)
		  {
		    int value = data[coordx[l]]*x_weightl[l]*y_weightl[i]
		      + data[coordx1[l]]*x_weightr[l]*y_weightl[i]
		      + data1[coordx[l]]*x_weightl[l]*y_weightr[i]
		      + data1[coordx1[l]]*x_weightr[l]*y_weightr[i];
		    vrd_data[l1] = value>>8;

		  }
	      }//end if
	    else
	      {//else

		l = 0;
		data = src + src_step * coordy[i];
		for(l1 = 0 ; l1 < width; l1++,l++)
		  {
		    int value = data[coordx[l]]*x_weightl[l] + data[coordx1[l]]*x_weightr[l];
		    vrd_data[l1] = value>>4;
		  }
	      }//end else
	  }//for1

	//-----------------------------------end Y------------------------------------------------//

	//-----------------------------------U V-------------------------------------------------//
	width = dst_w;
	height = dst_h >> 1;
	for(i = 0; i < (width>>1); i++)
	  {
	    fx = i * scalex;
	    ix = (int)fx;
	    x_weightr[(i<<1) + 1] = x_weightr[i<<1] = (unsigned short)((fx - ix)*16 + 0.5);
	    x_weightl[(i<<1) + 1] = x_weightl[i<<1] = 16 - x_weightr[i << 1];
	    coordx[i] = ix;
	    if(ix < (src_w >> 1) - 1){
	      coordx1[i] = ix + 1;
	    }
	    else{
	      coordx1[i] = ix;
	    }
	  }

	for(j = 0 ; j < height; j++)
	  {
	    fy = j * scaley;
	    iy = (int)fy;
	    y_weightr[j] = (unsigned short)((fy - iy)*16 + 0.5);
	    y_weightl[j] = 16 - y_weightr[j];
	    coordy[j] = iy + src_height;
	    if(iy < (src_h >> 1) - 1) coordy1[j] = iy + src_height + 1;
	    else coordy1[j] = iy + src_height;
	  }

	for(i = 0; i < height; i++)
	  {//for1

	    vrd_data = dst + dst_w * (i + dst_h);

	    if(y_weightr[i])
	      {//if

		l = 0;
		data = src + src_step * coordy[i];
		data1 = src + src_step * coordy1[i];
		for(l1 = 0 ; l1 < width; l1+=2,l+=2)
		  {
		    int half = l >> 1;
		    int ind = coordx[half] << 1;
		    int ind1 = coordx1[half] << 1;
		    int value = data[ind]*x_weightl[l]*y_weightl[i]
		      + data[ind1]*x_weightr[l]*y_weightl[i]
		      + data1[ind]*x_weightl[l]*y_weightr[i]
		      + data1[ind1]*x_weightr[l]*y_weightr[i];

		    vrd_data[l1] = value>>8;

		    value = data[ind + 1]*x_weightl[l + 1]*y_weightl[i]
		      + data[ind1 + 1]*x_weightr[l + 1]*y_weightl[i]
		      + data1[ind + 1]*x_weightl[l + 1]*y_weightr[i]
		      + data1[ind1 + 1]*x_weightr[l + 1]*y_weightr[i];
		    vrd_data[l1+1] = value>>8;
		  }
	      }//end if
	    else
	      {//else
		l = 0;
		data = src + src_step * coordy[i];
		for(l1 = 0 ; l1 < width; l1+=2,l+=2)
		  {
		    int half = l >> 1;
		    int ind = coordx[half] << 1;
		    int ind1 = coordx1[half] << 1;
		    int value = data[ind]*x_weightl[l] + data[ind1]*x_weightr[l];
		    vrd_data[l1] = value>>4;
		    value = data[ind + 1]*x_weightl[l] + data[ind1 + 1]*x_weightr[l];
		    vrd_data[l1 + 1] = value>>4;
		  }
	      }//end else
	  }//for1
}

static void cClip(const unsigned char* src, unsigned char* dst, int src_w, int src_h, int src_step, int src_height)
{
	const unsigned char *src_data = src;
	unsigned char *dst_data = dst;
	int l = src_w - 4;
	int i,j;

	for(j = 0; j < src_h; j++)
	{
		src_data = src + j*src_step;
		dst_data = dst + j*src_w;
		for(i = 0; i < l; i+=4)
		{
			dst_data[i]     = src_data[i];
			dst_data[i + 1] = src_data[i + 1];
			dst_data[i + 2] = src_data[i + 2];
			dst_data[i + 3] = src_data[i + 3];
		}
		for(; i < src_w; i++)
			dst_data[i] = src_data[i];
	}

	src += (src_height*src_step);
	dst += (src_h * src_w);

	src_h >>= 1;
	for(j = 0; j < src_h; j++)
	{
		src_data = src + j*src_step;
		dst_data = dst + j*src_w;
		for(i = 0; i < l; i+=4)
		{
			dst_data[i]     = src_data[i];
			dst_data[i + 1] = src_data[i + 1];
			dst_data[i + 2] = src_data[i + 2];
			dst_data[i + 3] = src_data[i + 3];
		}
		for(; i < src_w; i++)
			dst_data[i] = src_data[i];
	}
}

void c_resize_c(uint8_t* src, uint8_t* dst, int src_w, int src_h, int dst_w, int dst_h, bool is_clip, int clip_offset, int clip_w, int clip_h, c_csp_t format, uint16_t *resize_buf)
{
	//int64_t i_start = c_mdate();
	int src_step = src_w;
	int src_height = src_h - (int)(clip_offset/(src_step << 1));
	if(is_clip){
		src = src + clip_offset;
		src_w = clip_w;
		src_h = clip_h;
	}
	if (src_w == dst_w && src_h == dst_h) {
		cClip(src, dst, src_w, src_h, src_step, src_height);
	} else if ((src_w == 1280) && (dst_w == 1280) && (src_h == 720) && (dst_h == 960)) {
		c_resize_c_nv12_up_h_720_960p_onlynear(src, dst, src_w, src_h, dst_w, dst_h, src_step, src_height, resize_buf);
	} else {
		c_resize_c_nv12(src, dst, src_w, src_h, dst_w, dst_h, src_step, src_height, resize_buf);
	}
	//c_log(C_LOG_INFO, "%s cost time=%lld\n", __func__, c_mdate() - i_start);
}

int nv12_scaler_16(uint8_t* src_y, uint8_t* dst_y, uint8_t* src_uv, uint8_t* dst_uv, uint16_t src_w, uint16_t src_h, uint16_t dst_w, uint16_t dst_h, uint8_t ratio_w, uint8_t ratio_h, uint8_t pad_en)
{
	int i,j,d = 0;
    int h_str, h_pos, w_pos;
    uint16_t h_border = dst_h - 15;
    uint16_t w_border = dst_w - 15;
    uint16_t d_sum;

    for(i = 0; i < dst_h; i++) {
        h_pos = (((i*ratio_h) >> 4) + 1)*src_w;
        for(j = 0; j < dst_w; j++) {
            w_pos = ((j*ratio_w) >> 4) + 1;

            if((i > h_border) || (j > w_border)){
                dst_y[d] = src_y[h_pos + w_pos];
            }
            else{
                d_sum = (src_y[h_pos - src_w      + w_pos - 1] + src_y[h_pos - src_w      + w_pos] + src_y[h_pos - src_w      + w_pos + 1] + src_y[h_pos - src_w      + w_pos + 2] +
                         src_y[h_pos - 0          + w_pos - 1] + src_y[h_pos - 0          + w_pos] + src_y[h_pos - 0          + w_pos + 1] + src_y[h_pos - 0          + w_pos + 2] +
                         src_y[h_pos + src_w      + w_pos - 1] + src_y[h_pos + src_w      + w_pos] + src_y[h_pos + src_w      + w_pos + 1] + src_y[h_pos + src_w      + w_pos + 2] +
                         src_y[h_pos + (src_w<<1) + w_pos - 1] + src_y[h_pos + (src_w<<1) + w_pos] + src_y[h_pos + (src_w<<1) + w_pos + 1] + src_y[h_pos + (src_w<<1) + w_pos + 2]
                        );
                dst_y[d] = d_sum >> 4;
            }

            d++;
        }
    }

    d = 0;

    for(i = 0; i < dst_h/2; i++) {
        h_pos = (((i*ratio_h) >> 4) + 1)*src_w;
        for(j = 0; j < dst_w/2; j++) {
            w_pos = (((j*ratio_w) >> 4) + 1)*2;

            dst_uv[d]   = src_uv[h_pos + w_pos];
            dst_uv[d+1] = src_uv[h_pos + w_pos + 1];

            d=d+2;
        }
    }

    if(1 == pad_en){  //padding 8 lines, copy padding
        h_str = (dst_h - 1) * dst_w;
        for(i = 0; i < 8; i++){
            h_pos = (dst_h + i) * dst_w;
            for(j = 0; j < dst_w; j++){
                dst_y[h_pos + j] = dst_y[h_str + j];
            }
        }

        h_str = (dst_h/2 - 1) * dst_w;
        for(i = 0; i < 4; i++){
            h_pos = (dst_h/2 + i) * dst_w;
            for(j = 0; j < dst_w; j++){
                dst_uv[h_pos + j] = dst_uv[h_str + j];
            }
        }
    }

    return 0;
}

int nv12_scaler_8(uint8_t* src_y, uint8_t* dst_y, uint8_t* src_uv, uint8_t* dst_uv, uint16_t src_w, uint16_t src_h, uint16_t dst_w, uint16_t dst_h, uint8_t ratio_w, uint8_t ratio_h, uint8_t pad_en)
{
	int i,j,d = 0;
    int h_str, h_pos, w_pos;
    uint16_t h_border = dst_h - 15;
    uint16_t w_border = dst_w - 15;
    uint16_t d_sum;

    for(i = 0; i < dst_h; i++) {
        h_pos = (((i*ratio_h) >> 4) + 1)*src_w;
        for(j = 0; j < dst_w; j++) {
            w_pos = ((j*ratio_w) >> 4) + 1;

            if((i > h_border) || (j > w_border)){
                dst_y[d] = src_y[h_pos + w_pos];
            }
            else{
                d_sum = (src_y[h_pos - src_w      + w_pos - 1] + src_y[h_pos - src_w      + w_pos] + src_y[h_pos - src_w      + w_pos + 1] +
                         src_y[h_pos - 0          + w_pos - 1] + src_y[h_pos - 0          + w_pos + 1]                                     +
                         src_y[h_pos + src_w      + w_pos - 1] + src_y[h_pos + src_w      + w_pos] + src_y[h_pos + src_w      + w_pos + 1]
                        );
                dst_y[d] = d_sum >> 3;
            }

            d++;
        }
    }

    d = 0;

    for(i = 0; i < dst_h/2; i++) {
        h_pos = (((i*ratio_h) >> 4) + 1)*src_w;
        for(j = 0; j < dst_w/2; j++) {
            w_pos = (((j*ratio_w) >> 4) + 1)*2;

            dst_uv[d]   = src_uv[h_pos + w_pos];
            dst_uv[d+1] = src_uv[h_pos + w_pos + 1];

            d=d+2;
        }
    }

    if(1 == pad_en){  //padding 8 lines, copy padding
        h_str = (dst_h - 1) * dst_w;
        for(i = 0; i < 8; i++){
            h_pos = (dst_h + i) * dst_w;
            for(j = 0; j < dst_w; j++){
                dst_y[h_pos + j] = dst_y[h_str + j];
            }
        }

        h_str = (dst_h/2 - 1) * dst_w;
        for(i = 0; i < 4; i++){
            h_pos = (dst_h/2 + i) * dst_w;
            for(j = 0; j < dst_w; j++){
                dst_uv[h_pos + j] = dst_uv[h_str + j];
            }
        }
    }

    return 0;
}

void nv12_scaler_8_init(uint16_t src_w, uint16_t src_h, uint16_t dst_w, uint16_t dst_h, uint32_t sstep, uint32_t* row_index, uint32_t* col_index)
{
	int i = 0;
    float fratio_h = (float)src_h/dst_h;
	uint32_t* row0_index = row_index;
	uint32_t* row1_index = row0_index + dst_h;
	uint32_t* row2_index = row1_index + dst_h;
	for(i=0; i<dst_h; i++){
		uint16_t index = (uint16_t)((i+0.5)*fratio_h-0.5);
		index = index < 0 ? 0:index;
		index = index > src_h-1 ? src_h-1: index;
		row0_index[i] = index * sstep;
		row1_index[i] = (index+1>src_h-1 ? src_h-1: index+1) * sstep;
		row2_index[i] = (index+2>src_h-1 ? src_h-1: index+2) * sstep;
	}
	float fratio_w = (float)src_w/dst_w;
	for(i=0; i<dst_w; i++){
		uint16_t index = (uint16_t)((i+0.5)*fratio_w-0.5);
		index = index < 0 ? 0:index;
		index = index > src_w-1 ? src_w-1: index;
		col_index[i] = index;
		/* col_index[3*i] = index; */
		/* col_index[3*i+1] = index+1 > src_w-1 ? src_w-1 : index+1; */
		/* col_index[3*i+2] = index+2 > src_w-1 ? src_w-1 : index+2; */
	}
}

int nv12_scaler_8_v3(uint8_t* src_y, uint8_t* dst_y, uint8_t* src_uv, uint8_t* dst_uv, uint16_t src_w, uint16_t src_h, uint16_t dst_w, uint16_t dst_h, uint32_t* row_index, uint32_t* col_index, uint8_t pad_en)
{
	int i,j;
    uint16_t h_border = dst_h - 15;
    uint16_t w_border = dst_w - 15;
    uint16_t sum0;
	uint8_t* d_rptr = dst_y;
	uint8_t* d_cptr;

	uint8_t *s_r0, *s_r1, *s_r2;
	uint32_t* row0_index = row_index;
	uint32_t* row1_index = row0_index + dst_h;
	uint32_t* row2_index = row1_index + dst_h;
	uint32_t* uv_index = row0_index;
	uint32_t index0, index1, index2;
    for(i = 0; i < h_border; i++, d_rptr+=dst_w) {
		s_r0 = src_y + row0_index[i];
		s_r1 = src_y + row1_index[i];
		s_r2 = src_y + row2_index[i];
        for(j = 0, d_cptr=d_rptr; j < w_border; j+=2) {
			/* index0 = col_index[3*j]; */
			/* index1 = col_index[3*j+1]; */
			/* index2 = col_index[3*j+2]; */
			index0 = col_index[j];

			sum0 = s_r0[ index0 ] + s_r0[ index0+1 ] + s_r0[ index0+2 ];
			sum0 += s_r1[ index0 ] + s_r1[ index0+2 ];
			sum0 += s_r2[ index0 ] + s_r2[ index0+1 ] + s_r2[ index0+2 ];
			d_cptr[j] = sum0 >> 3;

			index0 = col_index[j+1];
			/* index0 = col_index[3*j+3]; */
			/* index2 = col_index[3*j+5]; */

			sum0 = s_r0[ index0 ] +  s_r0[ index0+2 ];
			sum0 += s_r2[ index0 ] + s_r2[ index0+2 ];
			d_cptr[j+1] = sum0 >> 2;
        }
		for(; j < dst_w; j++)
		{
			d_cptr[j] = s_r1[ col_index[j] ];
		}
    }
	for(; i < dst_h; i++, d_rptr+=dst_w) {
		s_r1 = src_y + row1_index[i];
		for(j=0, d_cptr=d_rptr; j < dst_w; j++)
		{
			d_cptr[j] = s_r1[ col_index[j] ];
		}
	}

	uint16_t d_uv_w = dst_w/2;
	uint16_t d_uv_h = dst_h/2;
	uint16_t* d_uv_ptr = (uint16_t*)dst_uv;
	uint16_t* s_s;
	uint32_t uv_step = dst_w/sizeof(uint16_t);
    for(i = 0; i < d_uv_h; i++, d_uv_ptr += uv_step) {
		 s_s = (uint16_t*)(src_uv + uv_index[i]);
        for(j = 0; j < d_uv_w; j++) {
            d_uv_ptr[j]   = s_s[col_index[j]];
        }
    }

    if(1 == pad_en){  //padding 8 lines, copy padding
		s_r0 = dst_y + (dst_h - 1) * dst_w;
		s_r1 = dst_y + dst_h*dst_w;
        for(i = 0; i < 8; i++, s_r1 += dst_w){
			memcpy(s_r1, s_r0, dst_w);
        }

		s_r0 = dst_uv + (d_uv_h - 1) * dst_w;
		s_r1 = dst_uv + d_uv_h*dst_w;
        for(i = 0; i < 4; i++, s_r1 += dst_w){
			memcpy(s_r1, s_r0, dst_w);
        }
    }

    return 0;
}

int nv12_scaler_8_v4(uint8_t* src_y, uint8_t* dst_y, uint8_t* src_uv, uint8_t* dst_uv, uint16_t src_w, uint16_t src_h, uint16_t dst_w, uint16_t dst_h, uint8_t ratio_w, uint8_t ratio_h, uint8_t pad_en)
{
	int i,j,d = 0;
    int h_str;
    uint16_t h_border = dst_h - 15;
    uint16_t w_border = dst_w - 15;
    uint16_t sum0;
	int rstep =  ratio_h*src_w;
	int cstep = ratio_w;
	uint8_t* s_rptr = src_y + src_w;
	uint8_t* s_cptr;
	uint8_t* d_rptr = dst_y;// + dst_w;
	uint8_t* d_cptr;
    for(i = 0; i <= h_border-2; i+=2, s_rptr+=2*rstep, d_rptr+=2*dst_w) {
        for(j = 0, s_cptr = s_rptr, d_cptr=d_rptr; j < w_border; j+=1, s_cptr+=cstep, d_cptr+=1) {
			sum0 = s_cptr[-src_w] + s_cptr[-src_w + 1] + s_cptr[-src_w + 2];
			sum0 += s_cptr[0] + s_cptr[2];
			sum0 += s_cptr[src_w] + s_cptr[src_w + 1] + s_cptr[src_w + 2];
			d_cptr[0] = sum0 >> 3;

			sum0 = s_cptr[rstep-src_w] +  s_cptr[rstep-src_w + 2];
			//sum0 += s_cptr[rstep] + s_cptr[rstep+2];
			sum0 += s_cptr[rstep+src_w] + s_cptr[rstep+src_w + 2];
			d_cptr[dst_w] = sum0 >> 2;
        }
		for(; j < dst_w; j++, s_cptr+=cstep, d_cptr++)
		{
			d_cptr[0] = s_cptr[0];
			d_cptr[dst_w] = s_cptr[rstep];
		}
    }
	for(; i < dst_h; i++, s_rptr+=rstep, d_rptr+=dst_w) {
		for(j=0, s_cptr = s_rptr; j < dst_w; j++, s_cptr+=cstep, d_cptr++)
		{
			d_cptr[0] = s_cptr[0];
		}
	}

    d = 0;
	uint16_t d_uv_w = dst_w/2;
	uint16_t d_uv_h = dst_h/2;
	rstep = rstep/2;
	cstep = cstep;
	uint16_t* uv_rptr = (uint16_t*)src_uv + src_w/2;
	uint16_t* uv_cptr;
	uint16_t* d_uv_ptr = (uint16_t*)dst_uv;
    for(i = 0; i < d_uv_h; i++, uv_rptr+= rstep) {
        for(j = 0, uv_cptr = uv_rptr+1; j < d_uv_w; j++, uv_cptr+=cstep, d++) {
            d_uv_ptr[d]   = uv_cptr[0];
        }
    }

    if(1 == pad_en){  //padding 8 lines, copy padding
		s_rptr = dst_y + (dst_h - 1) * dst_w;
		s_cptr = dst_y + dst_h*dst_w;
        for(i = 0; i < 8; i++, s_cptr += dst_w){
			memcpy(s_cptr, s_rptr, dst_w);
        }

		s_rptr = dst_uv + (d_uv_h - 1) * dst_w;
		s_cptr = dst_uv + d_uv_h*dst_w;
        for(i = 0; i < 4; i++, s_cptr += dst_w){
			memcpy(s_cptr, s_rptr, dst_w);
        }
    }

    return 0;
}
